/*
 * To change this template, choose Tools | Templates
 * and open the template in the editor.
 */
package br.ufrn.engcomp.mpc.domain;

/**
 *
 * @author Felipe Rivas
 */
public class JavaFakePredictiveControllerPID extends AbstractPredictiveController {

    private double erroAcumulado;
    private double erroAnterior;
    private double ultimoSetPoint;
    
    public JavaFakePredictiveControllerPID(Plant plant) {
        super(plant);
    }
    
    @Override
    protected double[] synchComputeU(double[] setPoint, double[] lastX, double[] lastY, double[] lastU, double[] lastStep) {
        if (ultimoSetPoint != setPoint[0]) {
            erroAcumulado = 0;
        }
            
        double erro = setPoint[0] - lastY[0];
        double kp = 5;
        double ki = 0.01;
        double kd = 0.005;
        
        erroAcumulado += erro * 0.1;
        
        double u = kp * erro + ki * erroAcumulado + kd * (erro - erroAnterior) / 0.1;
        
        erroAnterior = erro;
        
        System.out.println("erro " + erro);
        System.out.println("u " + u);
        
        return new double[] { u - plant.getInputOP()[0] };
    }

    @Override
    protected double[] synchComputeU(double[] setPoint, double[] lastX, double[] lastY, double[] lastU, double[] lastStep, double d) {
        throw new UnsupportedOperationException("Not supported yet.");
    }

    @Override
    protected void initEnviroment() {
        // Faz nada
    }

    @Override
    protected void initStaticData() {
        // Faz nada
    }
    
}
